#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <pthread.h>

#include "msg_queue.h"

#define LOG_TAG    "msg_queue"
#define LOG_LVL    ELOG_LVL_VERBOSE
#include <elog.h>

#define     LOG_D       log_d
#define     LOG_I       log_i
#define     LOG_W       log_w
#define     LOG_E       log_e

struct msg_queue *msg_queue_create(uint32_t msg_size, uint32_t msg_count)
{
    struct msg_queue *queue = (struct msg_queue*)malloc(sizeof(struct msg_queue));
    if (queue == NULL) {
        LOG_E("malloc struct msg_queue failed");
        return NULL;
    }

    if (pthread_mutex_init(&queue->mutex, NULL) != 0) {
        LOG_E("pthread_mutex_init failed");
        return NULL;
    }
    
    if (pthread_cond_init(&queue->full, NULL) != 0) {
        LOG_E("pthread_cond_init failed");
        return NULL;
    }

    if (pthread_cond_init(&queue->empty, NULL) != 0) {
        LOG_E("pthread_cond_init failed");
        return NULL;
    }

    queue->head = 0;
    queue->tail = 0;
    queue->count = 0;

    queue->msg_size = msg_size + (ALIGN_SIZE - 1) & ~(ALIGN_SIZE - 1);
    queue->msg_count = msg_count;
    LOG_D("msg_size: %u, msg_count: %u", queue->msg_size, queue->msg_count);
    
    queue->data = (char *)malloc(queue->msg_size * queue->msg_count);
    if (queue->data == NULL) {
        LOG_E("malloc data failed");
        return NULL;
    }
    memset(queue->data, 0, queue->msg_size * queue->msg_count);
    return queue;
}

int msg_queue_delete(struct msg_queue *queue)
{
    if (queue == NULL) {
        LOG_E("invalid parameters\n");
        return -1;
    }

    if (pthread_cond_destroy(&queue->empty) != 0) {
        LOG_E("pthread_cond_destroy failed");
    }

    if (pthread_cond_destroy(&queue->full) != 0) {
        LOG_E("pthread_cond_destroy failed");
    }

    if (pthread_mutex_destroy(&queue->mutex) != 0) {
        LOG_E("pthread_mutex_destroy failed");
    }

    if (queue->data != NULL) {
        free(queue->data);
        queue->data = NULL;
    }

    free(queue);
    queue = NULL;
    return 0;
}

int msg_queue_send(struct msg_queue *queue, uint8_t *msg, uint32_t msg_size)
{
    if (queue == NULL || msg == NULL || msg_size == 0) {
        LOG_E("invalid parameters");
        return -1;
    }

    pthread_mutex_lock(&queue->mutex);
    while(queue->count == queue->msg_count) {
        pthread_cond_wait(&queue->empty, &queue->mutex);
    }

    memcpy(queue->data + queue->tail * queue->msg_size, msg, msg_size);
    queue->tail = (queue->tail + 1) % queue->msg_count;
    queue->count ++;

    pthread_cond_signal(&queue->full);
    pthread_mutex_unlock(&queue->mutex);

    return 0;
}

int msg_queue_recv(struct msg_queue *queue, uint8_t *msg, uint32_t msg_size)
{
    if (queue == NULL || msg == NULL || msg_size == 0) {
        LOG_E("invalid parameters");
        return -1;
    }

    pthread_mutex_lock(&queue->mutex);
    while(queue->count == 0) {
        pthread_cond_wait(&queue->full, &queue->mutex);
    }

    memcpy(msg, queue->data + queue->head * queue->msg_size, msg_size);
    queue->head = (queue->head + 1) % queue->msg_count;
    queue->count --;

    pthread_cond_signal(&queue->empty);
    pthread_mutex_unlock(&queue->mutex);

    return 0;
}

#if 0
static struct msg_queue *q;
#define MSG_QUEUE_SIZE 100
#define MSG_SIZE 100

void* producer(void* arg)
{
    int i = 0;
    int ret = 0;
    while (i < 50) { // 生产50条消息
        #if 0
        pthread_mutex_lock(&q->mutex);
        while (q->count == MSG_QUEUE_SIZE) {
            pthread_cond_wait(&q->empty, &q->mutex);
        }
        sprintf(q->msgs[q->tail].data, "Message %d", i);
        q->msgs[q->tail].index = i;
        q->tail = (q->tail + 1) % MSG_QUEUE_SIZE;
        q->count++;
        pthread_cond_signal(&q->full);
        pthread_mutex_unlock(&q->mutex);
        #endif
        printf("producer i: %d\n", i);
        msg_queue_send(q, (uint8_t*)&i, sizeof(i));
        i++;
    }
    printf("========== Producer exited\n");
    return NULL;
}

void* consumer(void* arg)
{
    int ret = 0;
    int i;
    while (1) {
        #if 0
        pthread_mutex_lock(&q->mutex);
        while (q->count == 0) {
            pthread_cond_wait(&q->full, &q->mutex);
        }
        printf("Consumed: %d %s\n", q->msgs[q->head].index, q->msgs[q->head].data);
        q->head = (q->head + 1) % MSG_QUEUE_SIZE;
        q->count--;
        pthread_cond_signal(&q->empty);
        pthread_mutex_unlock(&q->mutex);
        #endif
        ret = msg_queue_recv(q, (uint8_t*)&i, sizeof(i));
        if (ret != 0) {
            printf("msg_queue_recv failed\n");
            break;
        }
        printf("Consumer i: %d\n", i);
    }
    printf("========== Consumer exited\n");
    return NULL;
}

int main()
{
    q = msg_queue_create(MSG_SIZE, MSG_QUEUE_SIZE);
    if (q == NULL) {
        printf("msg_queue_create failed\n");
        return -1;
    }

    pthread_t prod_tid, cons_tid;
    pthread_create(&prod_tid, NULL, producer, NULL);
    pthread_create(&cons_tid, NULL, consumer, NULL);

    pthread_join(prod_tid, NULL);
    printf("Producer join\n");

    pthread_join(cons_tid, NULL); // 取消消费者线程
    printf("Consumer cancelled\n");

    msg_queue_delete(q);

    return 0;
}
#endif